#include <string.h>

#include "StateMachine.h"
#include "usart.h"
#include "tick.h"
#include "timer.h"
#include "exit.h"
#include "mma7361.h"
#include "ds18b20.h"
#include "package.h"
#include "mma8451.h"
#include "battery.h"
#include "Bluetooth.h"
#include "stm32f10x.h"
#include "SensorChannels.h"

#undef TRACE_LEVEL
#undef TRACE_TITLE
#undef TRACE_TITLE
#define TRACE_LEVEL TRACE_LEVEL_DEBUG
#define TRACE_NAME  TRACE_NAME_DISABLE
#define TRACE_TITLE "STATE"
#include "trace.h"

static void InputPackageHandler(SensorXPackage_t *in);

#if (1)
static int SoftwarePackageInsert() {
	static int InsertOnce = 0;
	SensorXPackage_t package;
	uint16_t buffer[2];
	uint16_t channel = 2;
	
	if(InsertOnce == 0) {
		package.sync0 = 0;
		package.sync1 = 0;
		package.pkgLengthInWORD = 3;
		package.command = PKG_ENABLE_TIME;
		package.buffer = buffer;
		package.checkSum = 0x1234;
		
		buffer[0] = channel;
		InputPackageHandler(&package);
		InsertOnce = 1;
	}
}
#endif

// We never return from here
void PhySensorIsAlive(void) {
	int i;
	const int ATMode = 0;
	
	// register package handler
	RegisterPackageHandler(InputPackageHandler);
	
	// Init Bluetooth Control IO
	BluetoothCtrlIOInit();
	BluetoothReset(10);
	
	// Init the Battery Monitor
	BatteryMonitorInit(10000);
	
	if(ATMode) {
		BluetoothATMode();
		BluetoothReset(10);
		while(1);
	}
	
	// Open USART1
	USART_Open(USART, USART_BAUDRATE, USART_RX_BUFSIZE);
	
	while (1) {
		// listen in-coming packages
		PackageListener();
		
		// Debug Only
		// SoftwarePackageInsert();
		
		// External sensor data output
		for(i = 0; i < PHYSENSOR_CHANNELS; i ++) {
			if(isChannelEnable(i)) {
				OutputChannelData(i);
			}
		}
		
		// Internal sensor data output
		if(isGravitySensorEnable()) {
			OutputGravitySensorData();
		}
		
		// Battery Monitor data output
		OutBatterySensorData();
	}	
}

static void InputPackageHandler(SensorXPackage_t *in) {
	int i;
	char sum;
	char len;
	int error = 0;
	uint16_t disableMask = 0;
	uint16_t *CmdResultDataBuffer;
	uint16_t NoNeedMalloc[8];
	uint8_t bufferLength;
	int channel;
	
	sum = in->command;
	// data buffer len not include the checksum and command
	len = in->pkgLengthInWORD - 2; 
	
	if (in->command & (uint16_t)PKG_DISABLE_MASK) {
		disableMask = PKG_DISABLE_MASK;
		in->command &= ~((uint16_t)PKG_DISABLE_MASK);
	} else {
		disableMask = 0;
	}
	
	TRACE_DEBUG("PKG_CLASS=0x%x", in->command);
#if (TRACE_LEVEL >= TRACE_LEVEL_DEBUG)
	if(disableMask) {
		TRACE_DEBUG_WP("[DISABLE]");
	} else {
		TRACE_DEBUG_WP("[ENABLE]");
	}
#endif
	
	if (len) {
		TRACE_DEBUG("DATA[%d]{", len);
		for(i = 0; i < len; i ++) {
			TRACE_DEBUG_WP("%04x,", in->buffer[i]);
			sum += in->buffer[i];
		}
		TRACE_DEBUG_WP("}");
	} else {
		TRACE_DEBUG("NO DAT");
	}
	TRACE_DEBUG("SUM=0x%x", in->checkSum);
	
	if (sum != in->checkSum) {
		TRACE_DEBUG_WP("[FAILED<0x%04x>]", sum);
	} else {
		TRACE_DEBUG_WP("[PASSED]");
	}
	
	// no extra data as default
	CmdResultDataBuffer = NULL;
	
	switch(in->command) {
	case PKG_ENABLE_TEMP:
		channel = in->buffer[0];
		if (disableMask) {
			// Disable temperature sensor
			TRACE_DEBUG("Close temperature channel<%d>", channel);
			if(CloseChannel(channel) != 0)
				error = -1;
		} else {
			// Enable temperature sensor
			TRACE_DEBUG("Open temperature channel<%d>", channel);
			if(OpenChannel(channel, CHANNEL_TEMP) != 0)
				error = -1;			
		}
	break;
	case PKG_ENABLE_TIME:
		channel = in->buffer[0];
		if (disableMask) {
			// disable time sensor
			TRACE_DEBUG("Close time channel<%d>", channel);
			if(CloseChannel(channel) != 0)
				error = -1;
		} else {
			// enable time sensor
			TRACE_DEBUG("Open time channel<%d>", channel);
			if(OpenChannel(channel, CHANNEL_TIME) != 0)
				error = -1;
		}
	break;
	case PKG_ENABLE_FREQ:
		channel = in->buffer[0];
		if (disableMask) {
			// disable time sensor
			TRACE_DEBUG("Close freq channel<%d>", channel);
			if(CloseChannel(channel) != 0)
				error = -1;
		} else {
			// enable time sensor
			TRACE_DEBUG("Open freq channel<%d>", channel);
			if(OpenChannel(channel, CHANNEL_FREQ) != 0)
				error = -1;
		}
	break;
	case PKG_ENABLE_GRAVITY:
		if (disableMask) {
			TRACE_DEBUG("Close gravity sensor");
			if(CloseGravitySensor() != 0)
				error = -1;
		} else {
			TRACE_DEBUG("Open gravity sensor");
			if(OpenGrayvitySensor() != 0)
				error = -1;
		}
	break;
	case PKG_SELECT_GRAVITY_RANGE:
		TRACE_DEBUG("Select gravity sensor range");
		error = -1;
	break;
	case PKG_WRITE_GRAVITY_REG:
		TRACE_DEBUG("Write gravity sensor: 0x%x->reg[0x%x]", in->buffer[1] & 0xFF, in->buffer[0] & 0xFF);
		if(WriteGravitySensorRegister(in->buffer[0] & 0xFF, in->buffer[1] & 0xFF) != 0)
			error = -1;
	break;
	case PKG_READ_GRAVITY_REG:
		// command result is carring datas
		TRACE_DEBUG("Read gravity sensor: reg[0x%x]", in->buffer[0]);
		CmdResultDataBuffer = NoNeedMalloc;
		bufferLength = 1;
		
		error = ReadGravitySensorRegister(in->buffer[0]);
		if(error == -1) {
			CmdResultDataBuffer[0] = 0;
		} else {
			CmdResultDataBuffer[0] = error & 0xFF;
			error = 0;
		}
	break;
	case PKG_SET_SAMPLE_PERIOD:
		channel = in->buffer[0];
		TRACE_DEBUG("Set temp sample period %d millisecond", in->buffer[1]);
		if(SetTemperatureSensorReadPeriodInMillisecond(channel, in->buffer[1]) != 0)
			error = -1;
	break;
	case PKG_SET_GRAVITY_FREQ:
		TRACE_DEBUG("Set gravity sensor sampling frequency level %d", in->buffer[0]);
		if(SetGravitySamplingFreqency(in->buffer[0]) != 0)
			error = -1;
	default:
		error = -1;
	}

#if (TRACE_LEVEL >= TRACE_LEVEL_DEBUG)
	if(error == -1) {
		TRACE_DEBUG_WP("[FAILED]");
	} else {
		TRACE_DEBUG_WP("[PASSED]");
	}

	
	TRACE_DEBUG("Response Cmd[0x%x],error[%d]", in->command | disableMask, error);
	if(CmdResultDataBuffer) {
		TRACE_DEBUG("Response Data[%d]{", bufferLength);
	
		for(i = 0; i < bufferLength; i ++) {
			TRACE_DEBUG_WP("0x%x,", CmdResultDataBuffer[i]);
		}
		TRACE_DEBUG_WP("}");
	} else {
		TRACE_DEBUG("Response no extra data");
	}
#endif
	// Send response to usart
	WriteCmdResponse(in->command | disableMask, error, CmdResultDataBuffer, bufferLength);
}
